package com.robertbruce.astar;

import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.List;
import java.util.Map;


public class AStar<T extends Node<T>>
{

	public List<T> findCheapestPath(T theStart, T theTarget)
	{
		ArrayList<NodeState<T>> theOpenStates = new ArrayList<NodeState<T>>();
		Map<T, NodeState<T>> theOpenNodes = new HashMap<T, NodeState<T>>();
		Map<T, NodeState<T>> theClosedNodes = new HashMap<T, NodeState<T>>();
		
		// put the starting node in the open set
		NodeState<T> theStartingState = new NodeState<T>(theStart, null, 0, theTarget);
		addOpenNode(theStartingState, theOpenStates, theOpenNodes);
		
		// process all the open nodes.  We will add neighbors to the set and remove
		// traversed nodes from the set.  we should only break out of the loop
		// if there is no path to the target node.
		while (theOpenStates.isEmpty() == false)
		{
			// because the list is sorted the first element of the set is the cheapest
			NodeState<T> theCheapestNode = theOpenStates.get(0);
			theOpenStates.remove(theCheapestNode);
			theOpenNodes.remove(theCheapestNode.getNode());
			if (theTarget.equals(theCheapestNode.getNode()))
			{
				return theCheapestNode.calculatePath();
			}
			else
			{
				// ok so its not the goal add its neighbors to the open set
				for (T aNode : theCheapestNode.getNode().getNeighbors())
				{
					double theCostSoFar = theCheapestNode.getTheCostSoFar() + aNode.calculateEstimatedCost(theTarget);
					NodeState<T> aNewState = new NodeState<T>(aNode, theCheapestNode, theCostSoFar, theTarget);
					// is this node in the current open list and is it cheaper to go that way
					NodeState<T> aPossibleOpenNode = theOpenNodes.get(aNode);
					NodeState<T> aPossibleClosedNode = theClosedNodes.get(aNode);
					//
					// the node is not in the open or closed maps so add a new open node
					if (aPossibleOpenNode == null && aPossibleClosedNode == null)
					{
						// add a new node
						addOpenNode(aNewState, theOpenStates, theOpenNodes);
					}
					else if (aPossibleOpenNode != null && aPossibleOpenNode.getEstimatedCostToTarget() > theCostSoFar)
					{
						// the node is in the open map but the cost is more than our current cost
						// remove the current node and state and add the new, cheaper one.
						System.out.println("Swaping states");
						theOpenNodes.remove(aNode);
						theOpenStates.remove(aPossibleOpenNode);
						addOpenNode(aNewState, theOpenStates, theOpenNodes);
					}
					else if (aPossibleClosedNode != null && aPossibleClosedNode.getEstimatedCostToTarget() > theCostSoFar)
					{
						// we have seen this node before but the cost was higher.  Remove it from the closed list
						// and add the new cheaper cost node to the open list.
						System.out.println("Removing from closed nodes.");
						theClosedNodes.remove(aNode);
						addOpenNode(aNewState, theOpenStates, theOpenNodes);
					}					
				}
			}
			// add the traversed node to the closed collection
			theClosedNodes.put(theCheapestNode.getNode(), theCheapestNode);
			Collections.sort(theOpenStates);


		}
		// no path found
		return null;
	}
	
	private void addOpenNode(NodeState<T> theNodeState, ArrayList<NodeState<T>> theOpenStates, Map<T, NodeState<T>> theOpenNodes)
	{
		// add the node to the open list.  This is either a new never been seen before node
		// or a re-visited node from above.
		theOpenNodes.put(theNodeState.getNode(), theNodeState);
		theOpenStates.add(theNodeState);
	}
}

